import os
import launch
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.webots_launcher import WebotsLauncher
from webots_ros2_driver.webots_controller import WebotsController

'''
StartNodes:
    - webots:
        start webots simulator
    - robot_driver:
        start external controller

Tips:
    - robot_driver's topics are published in the [robot_name] namespace, if use input or core need remapping
'''

def generate_launch_description():
    package_dir = get_package_share_directory("robot_sim")
    robot_description_path = os.path.join(package_dir, "webots", "robot.urdf")

    webots = WebotsLauncher(world=os.path.join(package_dir, "webots", "mono.wbt"))

    my_robot_driver = WebotsController(
        robot_name="robot1",
        parameters=[
            {"robot_description": robot_description_path},
            {"direct_control": True},
            {"forward_scale": -5.0},
            {"rotation_scale": -30.0},
        ],
    )

    return LaunchDescription(
        [
            webots,
            my_robot_driver,
            launch.actions.RegisterEventHandler(
                event_handler=launch.event_handlers.OnProcessExit(
                    target_action=webots,
                    on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
                )
            ),
        ]
    )
